#include "FuzzyController_v1.2.hpp"



decision FuzzyController::getDecision(double target_pos, double obstacles_pos, double potential)
{
	decision ret_de; 

	//while (1)
	//{
	//	cout << "SELECT AN OPTION:\n\tS - set values\n\tQ - quit";
//		cout << endl;
//		cin >> option;

//		if (option == 'Q' || option == 'q')
//			break;

//		if (option == 'S' || option == 's')
//		{
/*			cout << "Target: " ;
			cin >> target_pos;
			cout << "Obstacles: "  ;
			cin >> obstacles_pos;
*/
			// set input variables...
			printf("targetpos = %0.2f, obsdir = %0.2f; potential = %0.2f", target_pos, obstacles_pos, potential);

	// math angle convention        // prog bug! put it in the wrong place
	//target_pos = 0;
	//obstacles_pos = -45;
	//potential = -10;

	// robot angle convention
	target_pos = -target_pos;
	obstacles_pos = -obstacles_pos;
	//potential = -1;

			// DEBUG DEFUZZIFYING BUG!!!!!!!
				int flg = 0;
			if ((target_pos == 0)&&((obstacles_pos == 44)||(obstacles_pos == 12))&&(potential < 0)) 
			{	
				flg = 1;
				if (obstacles_pos == 44)
					obstacles_pos = -44;
				else
					obstacles_pos = -12;
			}
			// end of heading debugging part 

			ffll_set_value(model_vel, child_vel, TARGET, target_pos);
			ffll_set_value(model_vel, child_vel, OBSTACLES, obstacles_pos);
			ffll_set_value(model_vel, child_vel, POTENTIAL, potential);


			//ffll_set_value(model_dir, child_dir, TARGET, (float)target_pos);
			//ffll_set_value(model_dir, child_dir, OBSTACLES, (float)obstacles_pos);
			//ffll_set_value(model_dir, child_dir, POTENTIAL, (float)potential);

			ffll_set_value(model_dir, child_dir, TARGET, target_pos);
			ffll_set_value(model_dir, child_dir, OBSTACLES, obstacles_pos);
			ffll_set_value(model_dir, child_dir, POTENTIAL, potential);

			// get and display the output value
			//int output_vel = (int)ffll_get_output_value(model_vel, child_vel);  // what's output? if having 2 values? -> no solution: create 2 models
			//int output_dir = (int)ffll_get_output_value(model_dir, child_dir);
			double output_vel = ffll_get_output_value(model_vel, child_vel);  // what's output? if having 2 values? -> no solution: create 2 models
			double output_dir = ffll_get_output_value(model_dir, child_dir);
			//float output_vel = (float)ffll_get_output_value((float)model_vel, (float)child_vel);  // what's output? if having 2 values? -> no solution: create 2 models
			//float output_dir = (float)ffll_get_output_value((float)model_dir, (float)child_dir);

			
			// DEBUG DEFUZZIFYING BUG!!!!!!!
			if (flg) 
			{	
				output_dir = -output_dir;
			}
			// end of heading debugging part 
			
			
			
			
			
			ret_de.direction = -output_dir; // convert to math angle convention
			ret_de.velocity = output_vel;
			//ret_de.velocity = 1;
			//printf("    steer=%0.4f; vel=%0.2f\n", ret_de.direction, ret_de.velocity);
			printf("    steer=%0.4f; vel=%0.2f\n", -output_dir, output_vel);
			//cout << "output: ";

			//cout << output;
/*
			switch(output)  // how to work if there are 2 values??
			{
			case (1):
				cout << "Run Away!";
				break;

			case (2):
				cout << "Fight Defensively";
				break;

			case (3):

				cout << "All Out Attack!";
				break;

			} // end switch

			cout << endl;
		} // end if option = 's'
*/
//	} // end while(1)

	return ret_de;

}

FuzzyController::FuzzyController(int& ret_val)
{
	//cout.setf(ios::fixed);
	//cout.precision(2); // only display 2 decimal places

	// create and load the model
	model_dir = ffll_new_model();
	model_vel = ffll_new_model();


	//int ret_val = (int)ffll_load_fcl_file(model, "..\\aiwisdom.fcl");
	int ret_val_dir = (int)ffll_load_fcl_file(model_dir, "fuzzycontroller_Steering.fcl");
	int ret_val_vel = (int)ffll_load_fcl_file(model_vel, "fuzzycontroller_Velocity.fcl");
	//int ret_val = (int)ffll_load_fcl_file(model, "aiwisdom.fcl");

	if ((ret_val_dir >= 0) && (ret_val_vel >= 0)) 
		ret_val = 1;
	else 
		ret_val = -1;
	
	// create a child for the model...
	if (ret_val >= 0) 
	{
		child_dir = ffll_new_child(model_dir);
		child_vel = ffll_new_child(model_vel);
	}
}


FuzzyController::~FuzzyController()
{

}